// odometry.h
#include "stm32f10x.h"                  // Device header
#include "clmotor.h"
#include "mpu6050.h"
#include "HMC5883L.h"
extern MPU6050 mpu;              // MPU6050 实例
extern HMC5883L mag;

#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif

#define TICKS_PER_REVOLUTION  1040
#define WHEEL_RADIUS_M        0.024f
#define WHEEL_CIRCUMFERENCE_M (2.0f * M_PI * WHEEL_RADIUS_M)
#define TICKS_TO_METER        (WHEEL_CIRCUMFERENCE_M / TICKS_PER_REVOLUTION)
#define WHEEL_BASE_M          0.138f  // 12.86cm

//小车位姿模块，更新并处理传感器数据，生成小车的位姿数据
typedef struct {
    // --- 传感器原始数据 ---
    int32_t encoder_left;     // 左轮编码器计数
    int32_t encoder_right;    // 右轮编码器计数
    float   gx,gy,gz;           // MPU6050 角速度 z 轴（rad/s）
    float   ax,ay,az; // 加速度
	float	mx,my,mz;

    // --- 实际物理量 ---
    float   linear_vel;       // 当前线速度 (m/s)
    float   angular_vel;      // 当前角速度 (rad/s)

    // --- 位姿估计（积分结果）---
    float   x;                // 全局坐标 x (m)
    float   y;                // 全局坐标 y (m)
    float   theta;            // 朝向角 (rad)，来自 IMU 或融合
	float	origin_azimuth;
	float	azimuth;
	
	float   distance_traveled;  // 自启动以来总路程（米）
	
	//传感器状态
	int mpu6050_error;
	int hmc5883l_error;

    // --- 其他 ---
    uint32_t timestamp;       // 最后更新时间（ms）
} OdometryData;

// 全局状态
extern OdometryData odom;

// 接口函数
void odometry_init(float _dt,SpeedSensor *_ss_left,SpeedSensor *_ss_right);
void odometry_update(void);          // 5ms 调用一次
void odometry_reset(float x, float y, float theta);           // 重置位姿
void odometry_reset_to_origin(void);
inline float odometry_get_distance(void) {
	return odom.distance_traveled;
}	// 自启动以来行驶距离
void odometry_print(void);
float wrap_angle(float angle);
